#include "util/ammath.h"

#include "amcostable.h"
#include "amsintable.h"

float AMMath::lusin(int deg) {
    return _SIN_TABLE[deg % 360];
}

float AMMath::lucos(int deg) {
    return _COS_TABLE[deg % 360];
}

__m128 AMMath::_mulVectorMatrix(sse_matrix m, __m128 v) {
    __m128 xra = _mm_mul_ps(_mm_replicate_x_ps(v), m.ra);
    __m128 yrb = _mm_mul_ps(_mm_replicate_y_ps(v), m.rb);
    __m128 zrc = _mm_mul_ps(_mm_replicate_z_ps(v), m.rc);
    __m128 wrd = _mm_mul_ps(_mm_replicate_w_ps(v), m.rd);

    __m128 result = _mm_add_ps(xra, yrb);
    result = _mm_add_ps(result, zrc);
    result = _mm_add_ps(result, wrd);

    return result;
}

float AMMath::_dot(__m128 v, __m128 u) {
    static float t[4] __attribute__((aligned(16))) = { 0.0f, 0.0f, 0.0f, 0.0f };
    __m128 vu = _mm_mul_ps(v, u);
    _mm_store_ps(&t[0], vu);
    float r = t[0] + t[1] + t[2] + t[3];
    return r;
}
